/***************************************************************************
 *            matrix.c
 *
 *  Sun Sep  7 00:39:52 2008
 *  Copyright  2008  Tiago Falcão
 *  <tiago@tiagofalcao.com>
 ****************************************************************************/

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Library General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor Boston, MA 02110-1301,  USA
 */
 
#include "matrix.h"

//Cria Matriz
GLfloat * matrix_create(){
	GLfloat *matrix;
	#ifdef _XMMINTRIN_H_INCLUDED			//Alocação de Dados Alinhados
		int i;
		matrix= _mm_malloc(16 * sizeof(GLfloat), 16);
		for(i=0;i<16;i++)
			matrix[i]=0.;
	#else									//Alocação de dados comuns
		matrix= calloc(16 , sizeof(GLfloat));
	#endif
	
	//Identidade
	matrix[0]=1.;
	matrix[5]=1.;
	matrix[10]=1.;
	matrix[15]=1.;
	
	return matrix;
}

//Deleta Matriz
void matrix_delete(GLfloat * matrix){
	#ifdef _XMMINTRIN_H_INCLUDED			//Libera dados alinhados
		_mm_free(matrix);
	#else									//Libera dados comuns
		free(matrix);
	#endif
}

//Multiplica Matriz x Matriz Transposta
void matrix_mult_trans(GLfloat * matrix, GLfloat * trans){
	#ifdef _XMMINTRIN_H_INCLUDED					//Multiplicacao com SSE
		__m128 m1,m2;
		GLfloat temp[4] __attribute__((aligned(16)));
		int i,j;
		int k=0;
		for(i=0;i<16;i+=4){
			m1=_mm_load_ps(&(matrix[i]));			//Carrega
			for(j=0;j<16;j+=4){
				m2=_mm_load_ps(&(trans[j]));		//Carrega
				m2=_mm_mul_ps(m1,m2);				//Multiplica
				#ifdef _PMMINTRIN_H_INCLUDED		//Soma com SSE3
					m2=_mm_hadd_ps(m2,m2);
					m2=_mm_hadd_ps(m2,m2);
					_mm_store_ss(&(matrix[k]),m2);
				#else								//Soma sem SSE3
					_mm_store_ps(temp,m2);
					matrix[k]=temp[0]+temp[1]+temp[2]+temp[4];
				#endif
				k++;
			}
		}
	#else											//Multiplicacao Normal
	//with *ERROR*
	int i,j,k,x;
	GLfloat t;
	for(i=0,x=0;i<16;i+=4){
		for(j=0;j<16;j+=4,x++){
			t=0.;
			for(k=0;k<4;k++)
				t+=matrix[i+k]*trans[j+k];
			matrix[x]=t;
		}
	}
	#endif
}
	
//Normaliza
void normalize(GLfloat v[3]) {    
   GLfloat d = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]); 
   if (d == 0.0) {
      //error("zero length vector");    
      v[0]=v[1]=v[2]=0.;
      return;
   }
   v[0] /= d; v[1] /= d; v[2] /= d; 
}

//Normal
void normcrossprod(GLfloat v1[3], GLfloat v2[3], GLfloat out[3]) 
{ 
   out[0] = v1[1]*v2[2] - v1[2]*v2[1]; 
   out[1] = v1[2]*v2[0] - v1[0]*v2[2]; 
   out[2] = v1[0]*v2[1] - v1[1]*v2[0]; 
   normalize(out);							//normaliza
}


//Determinante
GLfloat det(GLfloat v1[3],GLfloat v2[3],GLfloat v3[3]){
	return v1[0]*v2[1]*v3[2]+v1[1]*v2[2]*v3[0]+v1[2]*v2[0]*v3[1]
		-v1[2]*v2[1]*v3[0]-v1[1]*v2[0]*v3[2]-v1[0]*v2[2]*v3[1];
}



